#include "INS_task.h"
#include "QuaternionEKF.h"
#include "pid.h"
#include "BMI088driver.h"
#include "bsp_imu_pwm.h"

#define IMU_temp_PWM(pwm)  imu_pwm_set(pwm)

static void imu_temp_control(fp32 temp);
static void BodyFrameToEarthFrame(const fp32 *vecBF, fp32 *vecEF, fp32 *q);
static void EarthFrameToBodyFrame(const fp32 *vecEF, fp32 *vecBF, fp32 *q);
static void get_accel(fp32 gyro[3], fp32 accel[3]);

INS_t INS;
static pid_type_def imu_temp_pid;

static const fp32 imu_temp_PID[3] = {TEMPERATURE_PID_KP, TEMPERATURE_PID_KI, TEMPERATURE_PID_KD};
static pid_type_def imu_temp_pid;

const fp32 xb[3] = {1, 0, 0};
const fp32 yb[3] = {0, 1, 0};
const fp32 zb[3] = {0, 0, 1};

uint32_t INS_DWT_Count = 0;
static fp32 dt = 0;
static uint8_t first_temperate;

static fp32 INS_gyro[3] = {0.0f, 0.0f, 0.0f};
static fp32 INS_accel[3] = {0.0f, 0.0f, 0.0f};
static fp32 INS_quat[4] = {0.0f, 0.0f, 0.0f, 0.0f};
fp32 INS_angle[3] = {0.0f, 0.0f, 0.0f};

void INS_task(void const *pvParameters)
{
		IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
    INS.AccelLPF = 0.0085;
		PID_init(&imu_temp_pid, PID_POSITION, imu_temp_PID, TEMPERATURE_PID_MAX_OUT, TEMPERATURE_PID_MAX_IOUT);
		
    while(1)
    {
				const fp32 gravity[3] = {0, 0, 9.81f};
				dt = DWT_GetDeltaT(&INS_DWT_Count);
				BMI088_Read(&BMI088);
				get_accel(INS_gyro, INS_accel);
				INS.atanxz = -atan2f(INS_accel[0], INS_accel[2]);
				INS.atanyz = atan2f(INS_accel[1], INS_accel[2]);
				IMU_QuaternionEKF_Update(INS_gyro[0], INS_gyro[1], INS_gyro[2], INS_accel[0], INS_accel[1], INS_accel[2], dt, INS_angle + INS_YAW_ADDRESS_OFFSET, INS_angle + INS_PITCH_ADDRESS_OFFSET, INS_angle + INS_ROLL_ADDRESS_OFFSET);
				memcpy(INS_quat, QEKF_INS.q, sizeof(QEKF_INS.q));
				
				BodyFrameToEarthFrame(xb, INS.xn, INS_quat);
				BodyFrameToEarthFrame(yb, INS.yn, INS_quat);
				BodyFrameToEarthFrame(zb, INS.zn, INS_quat);
				fp32 gravity_b[3];
				EarthFrameToBodyFrame(gravity, gravity_b, INS_quat);
				for (uint8_t i = 0; i < 3; i++)
				{
						INS.MotionAccel_b[i] = (INS_accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
				}
				BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS_quat);
				imu_temp_control(BMI088.temperature);
				vTaskDelay(1);
    }
}

static void imu_temp_control(fp32 temp)
{
    uint16_t tempPWM;
    static uint8_t temp_constant_time = 0;
    if (first_temperate)
    {
        PID_calc(&imu_temp_pid, temp, 40.0f);
        if (imu_temp_pid.out < 0.0f)
        {
            imu_temp_pid.out = 0.0f;
        }
        tempPWM = (uint16_t)imu_temp_pid.out;
        IMU_temp_PWM(tempPWM);
    }
    else
    {
        if (temp > 40.0f)
        {
            temp_constant_time++;
            if (temp_constant_time > 200)
            {
                first_temperate = 1;
                imu_temp_pid.Iout = MPU6500_TEMP_PWM_MAX / 2.0f;
            }
        }

        IMU_temp_PWM(MPU6500_TEMP_PWM_MAX - 1);
    }
}

void BodyFrameToEarthFrame(const fp32 *vecBF, fp32 *vecEF, fp32 *q)
{
    vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] + (q[1] * q[2] - q[0] * q[3]) * vecBF[1] + (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
    vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] + (q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
    vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] + (q[2] * q[3] + q[0] * q[1]) * vecBF[1] + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}

void EarthFrameToBodyFrame(const fp32 *vecEF, fp32 *vecBF, fp32 *q)
{
    vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] + (q[1] * q[2] + q[0] * q[3]) * vecEF[1] + (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
    vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] + (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] + (q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
    vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] + (q[2] * q[3] - q[0] * q[1]) * vecEF[1] + (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}

void get_accel(fp32 gyro[3], fp32 accel[3])
{
		for (uint8_t i = 0; i < 3; i++)
		{
				gyro[i] = BMI088.gyro[i];
				accel[i] = BMI088.accel[i];
		}
}

const fp32 *get_INS_quat_point(void)
{
    return INS_quat;
}

const fp32 *get_INS_angle_point(void)
{
    return INS_angle;
}

extern const fp32 *get_gyro_data_point(void)
{
    return INS_gyro;
}

extern const fp32 *get_accel_data_point(void)
{
    return INS_accel;
}
